/*
 * Copyright (c) 2011-2013 Jarek Sacha. All Rights Reserved.
 *
 * Author's e-mail: jpsacha at gmail.com
 */
package br.com.manchini.stereocv.utils;

import com.googlecode.javacv.cpp.opencv_core;
import static com.googlecode.javacv.cpp.opencv_core.CV_32FC;
import com.googlecode.javacv.cpp.opencv_features2d;
import java.util.ArrayList;

public class MatcherUtils {

    /**
     * Convert from KeyPoint to Point2D32f representation
     */
    public static opencv_core.CvPoint2D32f[] toCvPoint2D32f(opencv_features2d.DMatch matches, opencv_features2d.KeyPoint keyPoints1, opencv_features2d.KeyPoint keyPoints2) {
        ArrayList<opencv_features2d.DMatch> matchess = new ArrayList<>();
        matchess.add(matches);
        return toCvPoint2D32f(matchess, keyPoints1, keyPoints2);
    }

    /**
     * Convert from KeyPoint to Point2D32f representation
     */
    public static opencv_core.CvPoint2D32f[] toCvPoint2D32f(ArrayList<opencv_features2d.DMatch> matches, opencv_features2d.KeyPoint keyPoints1, opencv_features2d.KeyPoint keyPoints2) {

        // Extract keypoints from each match, separate Left and Right
        int[] pointIndexes1 = new int[(matches.size())];
        int[] pointIndexes2 = new int[(matches.size())];
        for (int i = 0; i < matches.size(); i++) {
            pointIndexes1[i] = matches.get(i).queryIdx();
            pointIndexes2[i] = matches.get(i).trainIdx();
        }

        // Convert keypoints into Point2f
        opencv_core.CvPoint2D32f points1 = new opencv_core.CvPoint2D32f(matches.size());
        opencv_core.CvPoint2D32f points2 = new opencv_core.CvPoint2D32f(matches.size());
        opencv_features2d.KeyPoint.convert(keyPoints1, points1, pointIndexes1);
        opencv_features2d.KeyPoint.convert(keyPoints2, points2, pointIndexes2);

        return new opencv_core.CvPoint2D32f[]{points1, points2};
    }

    public static opencv_core.CvMat toCvMat(opencv_core.CvPoint2D32f points) {
        int oldPosition = points.position();
        opencv_core.CvMat m = opencv_core.CvMat.create(points.capacity(), 1, CV_32FC(2));
        for (int i = 0; i < points.capacity(); i++) {
            m.put(i, 0, 0, points.position(i).x());
            m.put(i, 0, 1, points.position(i).y());
        }
        points.position(oldPosition);
        return m;
    }

    public static int getIndexPontoMaisProximo(float x, float y, opencv_core.CvPoint2D32f pontos) {
        int index = 0;
        opencv_core.CvPoint2D32f atual = pontos.position(0);
        float menorDistancia = Math.abs(atual.x() - x) + Math.abs(atual.y() - y);

        for (int i = 0; i < pontos.capacity(); i++) {
            opencv_core.CvPoint2D32f position = pontos.position(i);
            float distancia = Math.abs(position.x() - x) + Math.abs(position.y() - y);
            if (distancia < menorDistancia) {
                index = i;
                menorDistancia = distancia;
            }
        }


        return index;

    }
}